% Script to create ground truth evaluation files for the real-world
% geometric registration benchmark, in the same spirit as Choi et al 2015. 
% 
% ---------------------------------------------------------
% Copyright (c) 2016, Andy Zeng
% 
% This file is part of the 3DMatch Toolbox and is available 
% under the terms of the Simplified BSD License provided in 
% LICENSE. Please retain this notice and LICENSE if you use 
% this file (or any portion of it) in your project.
% ---------------------------------------------------------

% Configuration options (change me)
sceneList = {'7-scenes-redkitchen', ...
             'sun3d-hotel_umd-maryland_hotel3', ...
             'sun3d-mit_76_studyroom-76-1studyroom2', ...
             'sun3d-mit_lab_hj-lab_hj_tea_nov_2_2012_scan1_erika', ...
             'sun3d-home_at-home_at_scan1_2013_jan_1', ...
             'sun3d-home_md-home_md_scan9_2012_sep_30', ...
             'sun3d-hotel_uc-scan3', ...
             'sun3d-hotel_umd-maryland_hotel1'};
fragmentsPath = '../../data/fragments';

for sceneIdx = 1:length(sceneList)

    scenePath = fullfile(fragmentsPath,sceneList{sceneIdx});
    numFragments = length(dir(fullfile(scenePath,'*.ply')));

    fidGtLog = fopen(fullfile([scenePath,'-evaluation'],'gt.log'),'w');
    fidGtInfo = fopen(fullfile([scenePath,'-evaluation'],'gt.info'),'w');

    for fragment1Idx = 0:(numFragments-1)
        for fragment2Idx = (fragment1Idx+1):(numFragments-1)

            fragment1Name = sprintf('cloud_bin_%d',fragment1Idx);
            fragment2Name = sprintf('cloud_bin_%d',fragment2Idx);

            fprintf('%s %s\n',fragment1Name,fragment2Name);

            % Load point clouds of both scene fragments and downsample to 1cm resolution
            fragment1PointCloud = pcread(fullfile(scenePath,sprintf('%s.ply',fragment1Name)));
            fragment2PointCloud = pcread(fullfile(scenePath,sprintf('%s.ply',fragment2Name)));
            fragment1PointCloud = pcdownsample(fragment1PointCloud,'gridAverage',0.01);
            fragment2PointCloud = pcdownsample(fragment2PointCloud,'gridAverage',0.01);

            % Get relative rigid transformation from fragment 2 to fragment 1
            % These .info files are generated by depth-fusion/fuseSceneFragments.m
            fragment1ExtCam2World = dlmread(fullfile([scenePath,'-info'],sprintf('%s.info.txt',fragment1Name)),'\t',[1,0,4,3]);
            fragment2ExtCam2World = dlmread(fullfile([scenePath,'-info'],sprintf('%s.info.txt',fragment2Name)),'\t',[1,0,4,3]);
            relExt = inv(fragment1ExtCam2World) * fragment2ExtCam2World;
            fragment2Points = fragment2PointCloud.Location';
            fragment2Points = relExt(1:3,1:3) * fragment2Points + repmat(relExt(1:3,4),1,size(fragment2Points,2));
%             pcwrite(pointCloud(fragment2Points','Color',repmat(uint8([255,0,0]),size(fragment2Points,2),1)),'debug1','PLYformat','binary');

            % Find overlapping surfaces
            voxelGridSize = 0.006;
            [nnIdx,nnSqrDist] = multiQueryKNNSearchImpl(fragment1PointCloud,fragment2Points',1);
            nnDist = sqrt(nnSqrDist);

            % Check if overlapping surface is over 30% of first fragment
            alignedRatio = sum(nnDist < voxelGridSize*5)/size(fragment1PointCloud.Location,1);
            if alignedRatio < 0.3
                continue;
            end

            % Compute 5000 ground truth correspondences q* on the overlapping regions
            corresQ = fragment2Points(:,find(nnDist < voxelGridSize))';
            if size(corresQ,1) > 5000
                corresQPointCloud = pcdownsample(pointCloud(corresQ),'random',5000/size(corresQ,1));
                corresQ = corresQPointCloud.Location;
            end
            % pcwrite(corresQPointCloud,'debug2','PLYformat','binary');

            % Compute covariance matrix from ground truth correspondence set
            covMat = zeros(6,6);
            for i = 1:size(corresQ,1)
                currQ = corresQ(i,:);
                Qx = [0,-currQ(3),currQ(2);currQ(3),0,-currQ(1);-currQ(2),currQ(1),0];
                G = [eye(3),-Qx];
                covMat = covMat + G'*G;
            end

            % Save gt.info and gt.log files
            fprintf(fidGtLog,'%d\t %d\t %d\t\n',fragment1Idx,fragment2Idx,numFragments);
            fprintf(fidGtLog,'%15.8e\t %15.8e\t %15.8e\t %15.8e\t\n',relExt');
            fprintf(fidGtInfo,'%d\t %d\t %d\t\n',fragment1Idx,fragment2Idx,numFragments);
            fprintf(fidGtInfo,'%15.8e\t %15.8e\t %15.8e\t %15.8e\t %15.8e\t %15.8e\t\n',covMat');

        end
    end

    fclose(fidGtLog);
    fclose(fidGtInfo);

end